//
// Created by Raymoond Tang on 19/4/13.
// Copyright (c) 2013 NUS. All rights reserved.
//
// To change the template use AppCode | Preferences | File Templates.
//


#include <boost/concept_check.hpp>
#include "CollisionRange.h"

//required: obstacle position and velocity (x,y), start and end point
CollisionRange::CollisionRange(Segment2D path, PlayerObject obstacle, double timeAllowed):tc(*this,0,timeAllowed),path(path),obstacle(obstacle),maxTime(timeAllowed) {
    //Vehicle Motion
    double xi1(path.start.x),yi1(path.start.y),xi2(path.end.x),yi2(path.end.y);
//        FloatVar xr(*this, x1, x2), yr(*this,y1,y2);
    double ai = yi1 - yi2,bi = xi2 - xi1,ci = yi2*xi1 - xi2*yi1;
    FloatVar xr(*this, std::min(path.start.x, path.end.x), std::max(path.start.x, path.end.x));
    FloatVar yr(*this, std::min(path.start.y, path.end.y), std::max(path.start.y, path.end.y));

    rel(*this,ai*xr + bi*yr + ci == 0);
    rel(*this, (xr - xi1) * (xr - xi2) <= 0);
    rel(*this, (yr - yi1) * (yr - yi2) <= 0);

    //obstMotion
//        xo =vjx ·(tc −tj−1)+xj−1
//        yo = vjy ·(tc −tj−1)+yj−1

    
    double xj1((obstacle.PosHighX + obstacle.PosLowX)/2),yj1((obstacle.PosHighY + obstacle.PosLowY)/2);
    double vjx(obstacle.VelX),vjy(obstacle.VelY),tj1(0.0);
    FloatVar xo(*this,-10,10),yo(*this,-10,10);

    rel(*this, vjx * (tc - tj1) + xj1 == xo);
    rel(*this, vjy * (tc - tj1) + yj1 == yo);
    //linear(Home home, const FloatValArgs& a, const FloatVarArgs& x,FloatRelType frt, FloatVar y);
    //collision
//        xrr ≥ x o + S x / 2
//        xr ≤ xo − Sx/2
//        yr ≥yo +Sy/2
//        yr ≤ yo − Sy/2
    double sx((obstacle.PosHighX - obstacle.PosLowX)*2), sy((obstacle.PosHighY - obstacle.PosLowY)*2);
//
    rel(*this, xr <= xo + sx/2);
    rel(*this, xr >= xo - sx/2);
    rel(*this, yr <= yo + sy/2);
    rel(*this, yr >= yo - sy/2);
}

CollisionRange::CollisionRange(bool share, CollisionRange & c):Space(share,c) {
    tc.update(*this, share, c.tc);
}

CollisionRange::~CollisionRange() {
    
}

std::pair<double, double>  CollisionRange::getRange() {
    DFS<CollisionRange> e(this);
    std::pair<double,double> result;
    if (CollisionRange* s = e.next()) {
        result.first = s->getMin();
        result.second = s->getMax();
        return result;
    }
    return result;
}

double CollisionRange::getMin() {
    return tc.min();
}

double CollisionRange::getMax() {
    return tc.max();
}
//int main(int argc, char* argv[]) {
//    // create model and search engine
////Path:start: (0.25 , -1.27) end: (0.215351 , -0.856653)
////
////Obstacle: 0
////    0.336452
////    -0.163548
////    -0.584277
////    -1.08427
////    0
////    0
////    cout << obstacles[0].PosHighX << endl;
////    cout << obstacles[0].PosLowX << endl;
////    cout << obstacles[0].PosHighY << endl;
////    cout << obstacles[0].PosLowY << endl;
////    cout << obstacles[0].VelX << endl;
////    cout << obstacles[0].VelY << endl;
//    Segment2D path(Robot2D(0,0),Robot2D(1, 1));
//    PlayerObject obstacle;
//    obstacle.PosLowY    = 0.81;
//    obstacle.PosHighY   = 1;
//    obstacle.PosLowX    = 0.163548;
//    obstacle.PosHighX   = 0.336452;
//    obstacle.VelX       = 0.2;
//    obstacle.VelY       = -0.1;
//    CollisionRange* range = new CollisionRange(path,obstacle,100.0);
//    std::pair<float,float> result(range->getRange());
//    std::cout << result.first << "," << result.second << std::endl;
//    return 0;
//}